#include <iostream>
#include <vector>
#include <queue>
#include <cmath>

#include "shape_approx.h"
#include "triangulate.h"

using namespace std;

const double EPS = 1e-6;

bool operator==(const MatrixElement &m1, const MatrixElement &m2)
{
	return m1.row == m2.row && m1.col == m2.col;
}

bool operator==(const Matrix &m1, const Matrix &m2)
{
	if (m1.Height() != m2.Height()
		|| m1.Width() != m2.Width())
	{
		return false;
	}

	for(int row = 0; row < m1.Height(); row ++)
		for(int col = 0; col < m1.Width(); col ++)
		{
			if (m1.at(row, col) != m2.at(row, col))
			{
				return false;
			}
		}

	return true;
}

bool operator==(const Point &p1, const Point &p2)
{
	return p1.x == p2.x && p1.y == p2.y;
}

bool operator==(const Shape &s1, const Shape &s2)
{
	return s1.points == s2.points;
}

inline vector<MatrixElement> get_relative_points(const Matrix &m, MatrixElement p, const int offsets[][2], int offsetsCount)
{	
	vector<MatrixElement> result;

	for(int i=0; i<offsetsCount; i++)
	{
		MatrixElement element(p.row + offsets[i][0], p.col + offsets[i][1]);
		if (m.IsInner(element))
		{
			result.push_back(element);
		}
	}

	return result;
}

inline vector<MatrixElement> get_neighbour_points(const Matrix &m, MatrixElement p)
{
	const int offsets[][2] = { {-1, -1}, {-1, 0}, {-1, 1}, {0, 1}, {1, 1}, {1, 0}, {1, -1}, {0, -1} };
	const int offsetsCount = 8;
	
	return get_relative_points(m, p, offsets, offsetsCount);
}

inline vector<MatrixElement> get_delate_points(const Matrix &m, MatrixElement p)
{
	const int offsets[][2] = { {-1, -1}, {-1, 0}, {0, -1} };
	const int offsetsCount = 3;
	
	return get_relative_points(m, p, offsets, offsetsCount);
}

inline vector<MatrixElement> get_special_case_delate_points(const Matrix &m, MatrixElement p)
{
	const int offsets[][2] = { {-2, 0}, {-1, 0}, {0, -2}, {0, -1} };
	const int offsetsCount = 4;
	
	return get_relative_points(m, p, offsets, offsetsCount);
}

inline vector<MatrixElement> filter_colored_points(const Matrix &m, vector<MatrixElement> points, int color)
{
	vector<MatrixElement> result;

	for(int i=0; i<(int)points.size(); i++)
	{
		if (m.at(points[i]) == color)
		{
			result.push_back(points[i]);
		}
	}
	
	return result;
}

void fill_background(Matrix &inner_input, MatrixElement startFrom)
{
	queue<MatrixElement> q;
	q.push(startFrom);

	while(!q.empty())
	{
		MatrixElement current = q.front();
		q.pop();

		vector<MatrixElement> moves = get_neighbour_points(inner_input, current);

		for(int i=0; i<(int)moves.size(); i++)
			if (inner_input.at(moves[i]) == 0)
			{
				inner_input.at(moves[i]) = -1;
				q.push(moves[i]);
			}
	}
}

void segment_shape(Matrix &inner_input, MatrixElement from, int color)
{
	queue<MatrixElement> q;
	q.push(from);

	while(!q.empty())
	{
		MatrixElement current = q.front();
		q.pop();

		vector<MatrixElement> moves = get_neighbour_points(inner_input, current);

		for(int i=0; i<(int)moves.size(); i++)
			if (inner_input.at(moves[i]) != -1 && inner_input.at(moves[i]) != color)
			{
				inner_input.at(moves[i]) = color;
				q.push(moves[i]);
			}
	}
}

vector<pair<int, MatrixElement>> segment_shapes(Matrix &inner_input)
{
	vector<pair<int, MatrixElement>> result;
	int color = 10;

	for(int row=0; row<inner_input.Height(); row++)
		for(int col=0; col<inner_input.Width(); col++)
		{
			if (inner_input.at(row, col) < 10 && inner_input.at(row, col) != -1)
			{
				result.push_back(make_pair(color, MatrixElement(row, col)));
				segment_shape(inner_input, MatrixElement(row, col), color ++);
			}
		}

	return result;
}

void delate(Matrix &input, vector<pair<int, MatrixElement>> &shapes)
{
	for(int row=0; row<input.Height(); row++)
		for(int col=0; col<input.Width(); col++)
			if (input.at(row, col) >= 10)
			{
				vector<MatrixElement> delatePoints;

				if (row > 0 && col > 0 
					&& input.at(row-1, col-1) == input.at(row, col)
					&& input.at(row-1, col) != input.at(row, col)
					&& input.at(row, col-1) != input.at(row, col))
				{
					delatePoints = get_special_case_delate_points(input, MatrixElement(row, col));
				}
				else
				{
					delatePoints = get_delate_points(input, MatrixElement(row, col));
				}

				for(int i=0; i<(int)delatePoints.size(); i++)
				{
					input.at(delatePoints[i]) = input.at(row, col);
				}
			}

	for(int i=0; i<(int)shapes.size(); i++)
	{
		MatrixElement current = shapes[i].second;
		vector<MatrixElement> candidates = get_delate_points(input, current);

		if (candidates.size() > 0)
		{
			shapes[i].second = candidates[0];
		}
	}
}

Point matrix_to_point(MatrixElement element)
{
	return Point(element.col, element.row);
}

Shape find_basic_shape(Matrix &input, MatrixElement currentPoint, MatrixElement lastPoint)
{
	Shape result;
	const int color = input.at(currentPoint);
	int iterations_left = 10000;

	MatrixElement current = currentPoint;
	MatrixElement last = lastPoint;

	result.points.push_back(matrix_to_point(current));

	while (true)
	{
		if (iterations_left-- < 0)
		{
			return Shape();
		}

		vector<MatrixElement> moves = filter_colored_points(
											input, 
											get_neighbour_points(input, current),
											color
											);

		if (moves.size() < 2)
		{
			return Shape();
		}

		int lastMove = -1;
		for(int i=0; i<(int)moves.size(); i++)
		{
			if (last == moves[i])
			{
				lastMove = i;
				break;
			}
		}

		if (lastMove == -1)
		{
			return Shape();
		}

		MatrixElement next = moves[(lastMove + 1) % moves.size()];

		if (next == currentPoint)
		{
			break;
		}

		result.points.push_back(matrix_to_point(next));

		last = current;
		current = next;
	}

	return result;
}

vector<Shape> find_basic_shapes(Matrix &input, vector<pair<int, MatrixElement>> &shapes_points)
{
	vector<Shape> result;

	for(int i=0; i<(int)shapes_points.size(); i++)
	{
		vector<MatrixElement> neighbour = filter_colored_points(
											input, 
											get_neighbour_points(input, shapes_points[i].second),
											input.at(shapes_points[i].second)
											);

		if (neighbour.size() < 2)
		{
			cerr << "Error building shape in " 
				<< shapes_points[i].second.col << "x"
				<< shapes_points[i].second.row << endl;

			continue;
		}

		Shape shape = find_basic_shape(input, shapes_points[i].second, neighbour[neighbour.size() - 1]);

		if (shape.points.size() > 0)
		{
			result.push_back(shape);
		}
		else
		{
			cerr << "Error building shape in " 
				<< shapes_points[i].second.col << "x"
				<< shapes_points[i].second.row << endl;
		}
	}

	return result;
}

double sqr(double a)
{
	return a * a;
}

double dist(Point p1, Point p2)
{
	return sqrt(sqr(p1.x - p2.x) + sqr(p1.y - p2.y));
}

vector<Shape> remove_trivial_vertexes(const vector<Shape> &shapes)
{
	vector<Shape> result;

	for(int i=0; i<(int)shapes.size(); i++)
	{
		Shape old_shape = shapes[i];
		Shape new_shape;

		new_shape.points.push_back(old_shape.points[0]);

		for(int vertex=1; vertex<(int)old_shape.points.size(); vertex++)
		{
			Point p1 = new_shape.points[new_shape.points.size() - 1];
			Point p2 = old_shape.points[vertex];
			Point p3 = old_shape.points[(vertex + 1) % old_shape.points.size()];

			/* If triangle is not degenerate, add point to new_shape*/
			if (fabs((double)
				(dist(p1, p2) + dist(p2, p3) - dist(p1, p3))
				) > EPS)
			{
				new_shape.points.push_back(p2);
			}
		}

		result.push_back(new_shape);
	}

	return result;
}

double square_triangle(Point p1, Point p2a, Point p3)
{
	double l1 = dist(p1, p2a);
	double l2 = dist(p3, p2a);
	double l3 = dist(p1, p3);
	double pp = (l1 + l2 + l3) / 2;
	double square_sqr = pp * (pp - l1) * (pp - l2) * (pp - l3);

	if (square_sqr <= 0)
	{
		return 0;
	}

	return sqrt(square_sqr);
}

vector<Shape> remove_trivial_vertexes_alternative(const vector<Shape> &shapes, double precision)
{
	vector<Shape> result;

	for(int i=0; i<(int)shapes.size(); i++)
	{
		Shape old_shape = shapes[i];
		Shape new_shape;

		new_shape.points.push_back(old_shape.points[0]);
		int last_vertex_num = 0;

		for(int vertex=1; vertex<(int)old_shape.points.size(); vertex++)
		{
			Point p1 = new_shape.points[new_shape.points.size() - 1];
			Point p2 = old_shape.points[vertex];
			Point p3 = old_shape.points[(vertex + 1) % old_shape.points.size()];

			for(int cv=last_vertex_num + 1; cv <= vertex; cv++) 
			{
				Point p2a = old_shape.points[cv];

				if (square_triangle(p1, p2a, p3) /*/ (dist(p1, p3) * dist(p1, p3))*/ > precision)
				{
					new_shape.points.push_back(p2);
					last_vertex_num = vertex;
					break;
				}
			}
		}

		result.push_back(new_shape);
	}

	return result;
}

void shift_shapes(vector<Shape> &shapes, double dx, double dy)
{
	for(int i=0; i<(int)shapes.size(); i++)
		for(int j=0; j<(int)shapes[i].points.size(); j++)
		{
			shapes[i].points[j].x += dx;
			shapes[i].points[j].y += dy;
		}
}

void fill_border(Matrix &input, int value)
{
	for(int row = 0; row<input.Height(); row++)
	{
		input.at(row, 0) = value;
		input.at(row, input.Width() - 1) = value;
	}

	for(int col = 0; col<input.Width(); col++)
	{
		input.at(0, col) = value;
		input.at(input.Height() - 1, col) = value;
	}
}

vector<Shape> find_shapes(const Matrix &input)
{
	Matrix inner_input = Matrix(input.Width()+2, input.Height()+2);

	for(int row=0; row<input.Height(); row++)
		for(int col=0; col<input.Width(); col++)
		{
			inner_input.at(row+1, col+1) = input.at(row, col);
		}

	fill_border(inner_input, 1);
	fill_background(inner_input, MatrixElement(1, 1));
	fill_border(inner_input, -1);

	vector<pair<int, MatrixElement>> segments = segment_shapes(inner_input);
	delate(inner_input, segments);
	vector<Shape> shapes = find_basic_shapes(inner_input, segments);
	shapes = remove_trivial_vertexes(shapes);

	shift_shapes(shapes, 0.5, 0.5);

	return shapes;
}

vector<Shape> triangulate(vector<Shape> shapes)
{
	vector<Shape> result;

	for(int i=0; i<(int)shapes.size(); i++)
	{
		Shape shape = shapes[i];

		Vector2dVector t;

		for(int j=0; j<(int)shape.points.size(); j++)
		{
			t.push_back(
				Vector2d(
					(float) shape.points[j].x, 
					(float) shape.points[j].y)
				);
		}

		Vector2dVector tresult;

		Triangulate::Process(t, tresult);

		int tcount = tresult.size() / 3;

		for(int j=0; j<tcount; j++)
		{
			Shape triangle;
			triangle.points.push_back( Point( tresult[3*j].GetX(), tresult[3*j].GetY() ) );
			triangle.points.push_back( Point( tresult[3*j + 1].GetX(), tresult[3*j + 1].GetY() ) );
			triangle.points.push_back( Point( tresult[3*j + 2].GetX(), tresult[3*j + 2].GetY() ) );

			result.push_back(triangle);
		}
	}

	return result;
}

vector<Shape> approximate(vector<Shape> shapes, double precision)
{
	return 
		triangulate(
			remove_trivial_vertexes_alternative(shapes, precision)
		);
}